#include <math.h>
#include <fstream>
#include <iostream>
#include <vector>
#include <opencv2/core.hpp>

#include "base/util.hh"

void expm(float r[3], float rm[9]) {
	cv::Matx33f H(
		0, -r[2], r[1],
		r[2], 0, -r[0],
		-r[1], r[0], 0);

	float angle2 = r[0] * r[0] + r[1] * r[1] + r[2] * r[2];
	float angle = sqrt(angle2);

	if (angle < 1E-14) {
		rm[0] = 1, rm[1] = 0, rm[2] = 0;
		rm[3] = 0, rm[4] = 1, rm[5] = 0;
		rm[6] = 0, rm[7] = 0, rm[8] = 1;
	}	else {
		float ef = sin(angle) / angle;
		float gee = (1.0f - cos(angle)) / angle2;
		cv::Matx33f mr = H * H* gee + H * ef + cv::Matx33f::eye();

		rm[0] = mr(0,0), rm[1] = mr(0,1), rm[2] = mr(0,2);
		rm[3] = mr(1,0), rm[4] = mr(1,1), rm[5] = mr(1,2);
		rm[6] = mr(2,0), rm[7] = mr(2,1), rm[8] = mr(2,2);
	}
}

void rvec2rotmat(const float r[3], float R[9]) {
	float t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t14, t16, t17, t18, t20, t22, t24, t27, t31, t33;

	t1 = r[0] * r[0];
	t2 = r[1] * r[1];
	t3 = r[2] * r[2];
	t4 = t1 + t2 + t3;

	if (t4 <= 1E-14) { // zero, R=I
		R[0] = R[4] = R[8] = 1.0;
		R[1] = R[2] = R[3] = R[5] = R[6] = R[7] = 0.0;
		return;
	}

	t5 = sqrt(t4);

#define SINCOS(x, sn, cs) do { *(sn)=sin(x); *(cs)=cos(x); } while (0)
	SINCOS(t5, &t14, &t6);
#undef SINCOS

	t7 = 1.0f - t6;
	t8 = 1 / t4;
	t9 = t8*t3;
	t10 = t8*t2;
	t16 = t14 / t5;
	t17 = t16*r[2];
	t18 = t7*t8;
	t20 = t18*r[1] * r[0];
	t22 = t16*r[1];
	t24 = t18*r[2] * r[0];
	t27 = t8*t1;
	t31 = t16*r[0];
	t33 = t18*r[2] * r[1];
	R[0] = 1.0f + t7*(-t9 - t10);
	R[1] = -t17 + t20;
	R[2] = t22 + t24;
	R[3] = t17 + t20;
	R[4] = 1.0f + t7*(-t9 - t27);
	R[5] = -t31 + t33;
	R[6] = -t22 + t24;
	R[7] = t31 + t33;
	R[8] = 1.0f + t7*(-t10 - t27);
}

cv::Matx44f ParseExpVecKarl(const std::string& pose_str) {
	std::vector<std::string> strs = tk::split(pose_str, ' ');

	CHECK(6 == strs.size()) << "ERROR: Invalid pose file";

	float rv[3];
	rv[0] = (float)std::atof(strs[3].c_str());
	rv[1] = (float)std::atof(strs[4].c_str());
	rv[2] = (float)std::atof(strs[5].c_str());

	float rm[9];
	//rvec2rotmat(rv, rm);
	expm(rv, rm);

	float t0 = (float)std::atof(strs[0].c_str());
	float t1 = (float)std::atof(strs[1].c_str());
	float t2 = (float)std::atof(strs[2].c_str());

	cv::Matx44f mat(
		rm[0], rm[1], -rm[2], t0,
		-rm[3], -rm[4], rm[5], -t1,
		rm[6], rm[7], -rm[8], t2,
		0, 0, 0, 1);

	return mat;
}

cv::Matx44f ParseExpVecRT(const std::string& pose_str) {
	std::vector<std::string> strs = tk::split(pose_str, ' ');

	CHECK(6 == strs.size()) << "ERROR: Invalid pose file";

	float rv[3];
	rv[0] = (float)std::atof(strs[0].c_str());
	rv[1] = (float)std::atof(strs[1].c_str());
	rv[2] = (float)std::atof(strs[2].c_str());

	float rm[9];
	rvec2rotmat(rv, rm);

	float t0 = (float)std::atof(strs[3].c_str());
	float t1 = (float)std::atof(strs[4].c_str());
	float t2 = (float)std::atof(strs[5].c_str());

	cv::Matx44f mat(
		rm[0], rm[1], rm[2], t0,
		rm[3], rm[4], rm[5], -t1,
		rm[6], rm[7], rm[8], t2,
		0, 0, 0, 1);

	return mat;
}

void ReadKarl(const std::string& pose_file, std::vector<cv::Matx44f>& poses) {
	std::ifstream fs;
	fs.open(pose_file);
	if (!fs.is_open()) {
		std::cerr << "failed to open pose file, press any key to exit" << std::endl;
		getchar();
		exit(-1);
	}

	std::string line;
	while (true) {
		if (fs.eof()) break;
		std::getline(fs, line);
		if (line.size() < 4) {
			std::cout << "pose file to short" << std::endl;
			break;
		}
		poses.push_back(ParseExpVecKarl(line));
	}
}

void ReadRT(const std::string& pose_file, std::vector<cv::Matx44f>& poses) {
	std::ifstream fs;
	fs.open(pose_file);
	if (!fs.is_open()) {
		std::cerr << "failed to open pose file, press any key to exit" << std::endl;
		getchar();
		exit(-1);
	}

	std::string line;
	while (true) {
		if (fs.eof()) break;
		std::getline(fs, line);
		if (line.size() < 4) {
			std::cout << "pose file to short" << std::endl;
			break;
		}
		poses.push_back(ParseExpVecKarl(line));
	}
}

void Write(const std::string& pose_file, std::vector<cv::Matx44f>& poses) {
	std::ofstream ofs_pose;
	ofs_pose.open(pose_file);
	CHECK(ofs_pose.is_open()) << "Cannot write the pose";

	for (int i = 0; i < poses.size(); ++i)
		ofs_pose
			<< poses[i](0, 0) << ' ' << poses[i](0, 1) << ' ' << poses[i](0, 2) << ' '
			<< poses[i](1, 0) << ' ' << poses[i](1, 1) << ' ' << poses[i](1, 2) << ' '
			<< poses[i](2, 0) << ' ' << poses[i](2, 1) << ' ' << poses[i](2, 2) << ' '
			<< poses[i](0, 3) << ' ' << poses[i](1, 3) << ' ' << poses[i](2, 3) << std::endl;

	ofs_pose.close();
}

int main() {
	std::vector<cv::Matx44f> poses;
	//ReadRT("data/dtd/exp/moving/gt_pose_exp.txt", poses);
	//Write("data/dtd/exp/moving/gt_pose.txt", poses);
	
	ReadKarl("data/karl/gt_pose_exp.txt", poses);
	Write("data/karl/gt_pose.txt", poses);

	std::cout << "you can " << std::endl;
	return 0;
}